#include <iostream>
#include <ros/ros.h>
#include <opencv2/opencv.hpp>



void Bresenham(cv::Point start, cv::Point end, cv::Mat &img)
{
    img = cv::Mat(800,800,CV_8UC1,1);
    // cv::imshow("img",img);
    // cv::waitKey(0);
    int dx = end.x - start.x;
    int dy = end.y - start.y;
    int ux = dx > 0 ? 1 : -1;
    int uy = dy > 0 ? 1 : -1;

    dx = abs(dx);
    dy = abs(dy);

    int x = start.x;
    int y = start.y;
    std::cout<<"dx:"<<dx<<" dy:"<<dy<<std::endl;

    if(dx>dy)
    {
        //x方向增量
        int e = -dx;
        //说明在中点上方
        for(int i = 0;i<dx;i++)
        {
            x += ux;
            e += 2*dy;
            if(e>=0)
            {
                //中点的上方
                y += uy;
                e -= 2*dx;
            }
            // img(x,y) = 255;
            img.at<int>(y,x) = 254;
            std::cout<<"x:"<<x<<" y:"<<y<<std::endl;
        }
    }
    else
    {

        int e = -dy;
        //说明在中点上方
        for(int i = 0;i<dy;i++)
        {
            y += uy;
            e += 2*dx;
            if(e>=0)
            {
                //中点的上方
                x += ux;
                e -= 2*dy;
            }
            // img(x,y) = 255;
            img.at<int>(y,x) = 254;
            std::cout<<"x:"<<x<<" y:"<<y<<std::endl;
        }
    }


    cv::imshow("img",img);
    cv::waitKey(0);

}

void Occ(const cv::Point& point)
{
    int x = point.x,y=point.y;

}


int main(int argc,char** argv)
{
    ros::init(argc,argv,"Bresenham");
    cv::Point start(100,100);
    
    cv::Point end(300,300);
    cv::Mat img;
    Bresenham(start,end,img);
    return 0;
}


